{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "try:\n",
    "  # We must install required packages if we are in Google Colab\n",
    "  import google.colab\n",
    "  %pip install roboticstoolbox-python>=1.0.2\n",
    "except:\n",
    "  # We are not in Google Colab\n",
    "  # Apply custon style to notebook\n",
    "  from IPython.core.display import HTML\n",
    "  import pathlib\n",
    "  styles_path = pathlib.Path(pathlib.Path().absolute(), \"style\", \"style.css\")\n",
    "  styles = open(styles_path, \"r\").read()\n",
    "  HTML(f\"<style>{styles}</style>\")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 2.0 Higher Order Derivatives\n",
    "\n",
    "$\\large{\\text{Manipulator Differential Kinematics}} \\\\ \\large{\\text{Part II: Acceleration and Advanced Applications}}$\n",
    "\n",
    "$\\text{By Jesse Haviland and Peter Corke}$\n",
    "\n",
    "<br>\n",
    "\n",
    "The sections of the Tutorial paper related to this notebook are listed next to each contents entry.\n",
    "It is beneficial to read these sections of the paper before attempting the notebook Section.\n",
    "\n",
    "### Contents\n",
    "\n",
    "[2.1 Higher Order Derivatives](#pre)\n",
    "* Part I of the Tutorial\n",
    "  * Deriving the Manipulator Jacobian\n",
    "  * First Derivative of an Elementary Transform\n",
    "  * The Manipulator Jacobian\n",
    "  * Fast Manipulator Jacobian\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# We will do the imports required for this notebook here\n",
    "\n",
    "# numpy provides import array and linear algebra utilities\n",
    "import numpy as np\n",
    "\n",
    "# the robotics toolbox provides robotics specific functionality\n",
    "import roboticstoolbox as rtb"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='pre'></a>\n",
    "### 2.1 Higher Order Derivatives\n",
    "---\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Starting from $n = 3$, obtaining the matrix of $n^{th}$ partial derivative of the manipulator kinematics can be obtained using the product rule on the $(n - 1)^{th}$ partial derivative while considering their partitioned form.\n",
    "\n",
    "For example, to obtain the $3^{rd}$ partial derivative, we take the partial derivative of the manipulator Hessian with respect to the joint coordinates, in its partitioned form\n",
    "\n",
    "\\begin{align*}\n",
    "    \\frac{\\partial \\bf{H}_{jk}(\\bf{q})}\n",
    "         {\\partial q_l}\n",
    "    &=\n",
    "    \\begin{pmatrix}\n",
    "        \\dfrac{\\partial \\bf{H}_{a_{jk}}(\\bf{q})}\n",
    "              {\\partial q_l} \\\\\n",
    "        \\dfrac{\\partial \\bf{H}_{\\alpha_{jk}}(\\bf{q})}\n",
    "              {\\partial q_l}\n",
    "    \\end{pmatrix}  \\\\\n",
    "    &=\n",
    "    \\begin{pmatrix}\n",
    "        \\dfrac{\\partial }\n",
    "              {\\partial q_l}\n",
    "        \\left( \\bf{J}_{\\omega_k} \\times \\bf{J}_{\\omega_j} \\right) \\\\\n",
    "        \\dfrac{\\partial}\n",
    "              {\\partial q_l}\n",
    "        \\left( \\bf{J}_{\\omega_k} \\times \\bf{J}_{v_j} \\right)\n",
    "    \\end{pmatrix}  \\\\\n",
    "    &=\n",
    "    \\begin{pmatrix}\n",
    "        \\left( \\bf{H}_{\\omega_{kl}} \\times \\bf{J}_{\\omega_j} \\right) +\n",
    "        \\left( \\bf{J}_{\\omega_k} \\times \\bf{H}_{\\omega_{jl}} \\right) \\\\\n",
    "        \\left( \\bf{H}_{\\omega_{kl}} \\times \\bf{J}_{v_j} \\right) +\n",
    "        \\left( \\bf{J}_{\\omega_k} \\times \\bf{H}_{v_{jl}} \\right)\n",
    "    \\end{pmatrix}\n",
    "\\end{align*}\n",
    "\n",
    "where $\\frac{\\partial \\bf{H}_{jk}(\\bf{q})}{\\partial q_l} \\in \\mathbb{R}^{6}$. Continuing, we obtain the following\n",
    "\n",
    "\\begin{align*}\n",
    "    \\frac{\\partial \\bf{H}_{jk}(\\bf{q})}\n",
    "         {\\partial \\bf{q}}\n",
    "    &=\n",
    "    \\begin{pmatrix}\n",
    "        \\dfrac{\\partial \\bf{H}_{jk}(\\bf{q})}\n",
    "            {\\partial q_0} & \\cdots & \n",
    "        \\dfrac{\\partial \\bf{H}_{jk}(\\bf{q})}\n",
    "            {\\partial q_n}\n",
    "    \\end{pmatrix}\n",
    "\\end{align*}\n",
    "\n",
    "where $\\frac{\\partial \\bf{H}_{jk}(\\bf{q})}{\\partial q_l} \\in \\mathbb{R}^{6 \\times n}$,\n",
    "\n",
    "\\begin{align*}\n",
    "    \\frac{\\partial \\bf{H}_{j}(\\bf{q})}\n",
    "        {\\partial \\bf{q}}\n",
    "    &=\n",
    "    \\begin{pmatrix}\n",
    "        \\dfrac{\\partial \\bf{H}_{j_0}(\\bf{q})}\n",
    "            {\\partial \\bf{q}} & \\cdots & \n",
    "        \\dfrac{\\partial \\bf{H}_{j_n}(\\bf{q})}\n",
    "            {\\partial \\bf{q}}\n",
    "    \\end{pmatrix}\n",
    "\\end{align*}\n",
    "\n",
    "where $\\frac{\\partial \\bf{H}_{jk}(\\bf{q})}{\\partial \\bf{q}} \\in \\mathbb{R}^{6 \\times n \\times n}$, and finally \n",
    "\n",
    "\\begin{align*}\n",
    "    \\frac{\\partial \\bf{H}(\\bf{q})}\n",
    "         {\\partial \\bf{q}}\n",
    "    &=\n",
    "    \\begin{pmatrix}\n",
    "        \\dfrac{\\partial \\bf{H}_{0}(\\bf{q})}\n",
    "            {\\partial \\bf{q}} & \\cdots & \n",
    "        \\dfrac{\\partial \\bf{H}_{n}(\\bf{q})}\n",
    "            {\\partial \\bf{q}}\n",
    "    \\end{pmatrix}\n",
    "\\end{align*}\n",
    "\n",
    "where $\\frac{\\partial \\bf{H}(\\bf{q})}{\\partial \\bf{q}} \\in \\mathbb{R}^{6 \\times n \\times n \\times n}$ is the 4-dimensional tensor representing the $3^{rd}$ partial derivative of the manipulator kinematics.\n",
    "\n",
    "Note that the function has $\\mathcal{O}(n^{order})$ time complexity, where $order$ represents the order of the partial derivative being calculated."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "def partial_fkine0(ets, q, n):\n",
    "    r\"\"\"\n",
    "    Manipulator Forward Kinematics nth Partial Derivative\n",
    "\n",
    "    :return: The nth Partial Derivative of the forward kinematics\n",
    "    \"\"\"\n",
    "\n",
    "    # Calculate the Jacobian and Hessian\n",
    "    J = ets.jacob0(q)\n",
    "    H = ets.hessian0(q)\n",
    "\n",
    "    # A list of derivatives, starting with the jacobian and hessian\n",
    "    dT = [J, H]\n",
    "\n",
    "    # The tensor dimensions of the latest derivative\n",
    "    # Set to the current size of the Hessian\n",
    "    size = [ets.n, 6, ets.n]\n",
    "\n",
    "    # An array which keeps track of the index of the partial derivative\n",
    "    # we are calculating\n",
    "    # It stores the indices in the order: \"j, k, l. m, n, o, ...\"\n",
    "    # where count is extended to match oder of the partial derivative\n",
    "    count = np.array([0, 0])\n",
    "\n",
    "    # The order of derivative for which we are calculating\n",
    "    # The Hessian is the 2nd-order so we start with c = 2\n",
    "    c = 2\n",
    "\n",
    "    def add_indices(indices, c):\n",
    "        total = len(indices * 2)\n",
    "        new_indices = []\n",
    "\n",
    "        for i in range(total):\n",
    "            j = i // 2\n",
    "            new_indices.append([])\n",
    "            new_indices[i].append(indices[j][0].copy())\n",
    "            new_indices[i].append(indices[j][1].copy())\n",
    "\n",
    "            if i % 2 == 0:\n",
    "                # if even number\n",
    "                new_indices[i][0].append(c)\n",
    "            else:\n",
    "                # if odd number\n",
    "                new_indices[i][1].append(c)\n",
    "\n",
    "        return new_indices\n",
    "\n",
    "    def add_pdi(pdi):\n",
    "        total = len(pdi * 2)\n",
    "        new_pdi = []\n",
    "\n",
    "        for i in range(total):\n",
    "            j = i // 2\n",
    "            new_pdi.append([])\n",
    "            new_pdi[i].append(pdi[j][0])\n",
    "            new_pdi[i].append(pdi[j][1])\n",
    "\n",
    "            # if even number\n",
    "            if i % 2 == 0:\n",
    "                new_pdi[i][0] += 1\n",
    "            # if odd number\n",
    "            else:\n",
    "                new_pdi[i][1] += 1\n",
    "\n",
    "        return new_pdi\n",
    "\n",
    "    # these are the indices used for the hessian\n",
    "    indices = [[[1], [0]]]\n",
    "\n",
    "    # The partial derivative indices (pdi)\n",
    "    # the are the pd indices used in the cross products\n",
    "    pdi = [[0, 0]]\n",
    "\n",
    "    # The length of dT correspods to the number of derivatives we have calculated\n",
    "    while len(dT) != n:\n",
    "\n",
    "        # Add to the start of the tensor size list \n",
    "        size.insert(0, ets.n)\n",
    "\n",
    "        # Add an axis to the count array\n",
    "        count = np.concatenate(([0], count))\n",
    "\n",
    "        # This variables corresponds to indices within the previous partial derivatives\n",
    "        # to be cross prodded\n",
    "        # The order is: \"[j, k, l, m, n, o, ...]\"\n",
    "        # Although, our partial derivatives have the order: pd[..., o, n, m, l, k, cartesian DoF, j]\n",
    "        # For example, consider the Hessian Tensor H[n, 6, n], the index H[k, :, j]. This corrsponds\n",
    "        # to the second partial derivative of the kinematics of joint j with respect to joint k.\n",
    "        indices = add_indices(indices, c)\n",
    "\n",
    "        # This variable corresponds to the indices in Td which corresponds to the \n",
    "        # partial derivatives we need to use\n",
    "        pdi = add_pdi(pdi)\n",
    "\n",
    "        c += 1\n",
    "\n",
    "        # Allocate our new partial derivative tensor\n",
    "        pd = np.zeros(size)\n",
    "\n",
    "        # We need to loop n^c times\n",
    "        # There are n^c columns to calculate\n",
    "        for _ in range(ets.n**c):\n",
    "            \n",
    "            # Allocate the rotation and translation components\n",
    "            rot = np.zeros(3)\n",
    "            trn = np.zeros(3)\n",
    "\n",
    "            # This loop calculates a single column ([trn, rot]) of the tensor for dT(x)\n",
    "            for j in range(len(indices)):\n",
    "                pdr0 = dT[pdi[j][0]]\n",
    "                pdr1 = dT[pdi[j][1]]\n",
    "\n",
    "                idx0 = count[indices[j][0]]\n",
    "                idx1 = count[indices[j][1]]\n",
    "\n",
    "                # This is a list of indices selecting the slices of the previous tensor\n",
    "                idx0_slices = np.flip(idx0[1:])\n",
    "                idx1_slices = np.flip(idx1[1:])\n",
    "\n",
    "                # This index selecting the column within the 2d slice of the previous tensor\n",
    "                idx0_n = idx0[0]\n",
    "                idx1_n = idx1[0]\n",
    "\n",
    "                # Use our indices to select the rotational column from pdr0 and pdr1\n",
    "                col0_rot = pdr0[(*idx0_slices, slice(3, 6), idx0_n)]\n",
    "                col1_rot = pdr1[(*idx1_slices, slice(3, 6), idx1_n)]\n",
    "\n",
    "                # Use our indices to select the translational column from pdr1\n",
    "                col1_trn = pdr1[(*idx1_slices, slice(0, 3), idx1_n)]\n",
    "\n",
    "                # Perform the cross product as described in the maths above\n",
    "                rot += np.cross(col0_rot, col1_rot)\n",
    "                trn += np.cross(col0_rot, col1_trn)\n",
    "\n",
    "            pd[(*np.flip(count[1:]), slice(0, 3), count[0])] = trn\n",
    "            pd[(*np.flip(count[1:]), slice(3, 6), count[0])] = rot\n",
    "\n",
    "            count[0] += 1\n",
    "            for j in range(len(count)):\n",
    "                if count[j] == ets.n:\n",
    "                    count[j] = 0\n",
    "                    if j != len(count) - 1:\n",
    "                        count[j + 1] += 1\n",
    "\n",
    "        dT.append(pd)\n",
    "\n",
    "    return dT[-1]"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Lets try it out"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Make a panda robot\n",
    "panda = rtb.models.Panda()\n",
    "\n",
    "# Get the ets of our robot\n",
    "ets = panda.ets()\n",
    "\n",
    "# Make a joint coordinate vector for the Panda\n",
    "q = [0.21, -0.03, 0.35, -1.90, -0.04, 1.96, 1.36]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(7, 7, 6, 7)\n"
     ]
    }
   ],
   "source": [
    "# Lets do the third-order differential dinematics\n",
    "pd3 = partial_fkine0(ets, q, 3)\n",
    "\n",
    "# When using the Panda, this will have dimensions 7 x 7 x 6 x 7\n",
    "print(pd3.shape)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[[[ 0.3  -0.08  0.3  ...  0.09 -0.17  0.  ]\n",
      "   [-0.49 -0.02 -0.49 ... -0.14 -0.11  0.  ]\n",
      "   [-0.    0.   -0.   ... -0.    0.    0.  ]\n",
      "   [ 0.    0.21  0.03 ... -0.81 -0.54 -0.1 ]\n",
      "   [ 0.   -0.98  0.01 ... -0.5   0.84 -0.01]\n",
      "   [ 0.   -0.   -0.   ...  0.    0.    0.  ]]\n",
      "\n",
      "  [[ 0.    0.11  0.   ...  0.   -0.02  0.  ]\n",
      "   [-0.   -0.53 -0.01 ... -0.01  0.1   0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.  ]\n",
      "   [-0.21  0.   -0.21 ...  0.06 -0.01  0.21]\n",
      "   [ 0.98  0.    0.98 ... -0.29  0.05 -0.97]\n",
      "   [ 0.    0.    0.   ...  0.    0.   -0.  ]]\n",
      "\n",
      "  [[ 0.3  -0.06  0.3  ...  0.09 -0.17  0.  ]\n",
      "   [-0.49 -0.01 -0.49 ... -0.14 -0.11  0.  ]\n",
      "   [-0.    0.   -0.   ... -0.    0.    0.  ]\n",
      "   [-0.03  0.21  0.   ... -0.8  -0.54 -0.07]\n",
      "   [-0.01 -0.98  0.   ... -0.5   0.84 -0.  ]\n",
      "   [ 0.   -0.    0.   ...  0.    0.    0.  ]]\n",
      "\n",
      "  ...\n",
      "\n",
      "  [[-0.09 -0.41 -0.09 ... -0.03  0.14  0.  ]\n",
      "   [ 0.14 -0.27  0.14 ...  0.04  0.09  0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.  ]\n",
      "   [ 0.81 -0.06  0.8  ...  0.    0.2  -0.78]\n",
      "   [ 0.5   0.29  0.5  ...  0.   -0.22 -0.5 ]\n",
      "   [-0.    0.   -0.   ...  0.   -0.    0.  ]]\n",
      "\n",
      "  [[ 0.01 -0.29  0.01 ... -0.    0.05  0.  ]\n",
      "   [-0.02  0.45 -0.02 ...  0.   -0.09  0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.  ]\n",
      "   [ 0.54  0.01  0.54 ... -0.2   0.   -0.54]\n",
      "   [-0.84 -0.05 -0.84 ...  0.22  0.    0.84]\n",
      "   [-0.    0.   -0.   ...  0.    0.    0.  ]]\n",
      "\n",
      "  [[-0.3   0.03 -0.3  ... -0.09  0.18  0.  ]\n",
      "   [ 0.48  0.01  0.49 ...  0.13  0.11  0.  ]\n",
      "   [ 0.   -0.    0.   ...  0.   -0.    0.  ]\n",
      "   [ 0.1  -0.21  0.07 ...  0.78  0.54  0.  ]\n",
      "   [ 0.01  0.97  0.   ...  0.5  -0.84  0.  ]\n",
      "   [-0.    0.   -0.   ... -0.   -0.    0.  ]]]\n",
      "\n",
      "\n",
      " [[[-0.08  0.11  0.   ...  0.   -0.02  0.  ]\n",
      "   [-0.02 -0.53 -0.01 ... -0.01  0.1   0.  ]\n",
      "   [ 0.   -0.   -0.   ... -0.    0.    0.  ]\n",
      "   [ 0.    0.   -0.21 ...  0.06 -0.01  0.21]\n",
      "   [ 0.    0.    0.98 ... -0.29  0.05 -0.97]\n",
      "   [ 0.   -0.    0.   ... -0.    0.   -0.  ]]\n",
      "\n",
      "  [[-0.   -0.08  0.19 ...  0.06 -0.18  0.  ]\n",
      "   [-0.   -0.02  0.04 ...  0.01 -0.04  0.  ]\n",
      "   [ 0.    0.54  0.01 ...  0.01 -0.11  0.  ]\n",
      "   [-0.    0.    0.03 ... -0.88 -0.35 -0.1 ]\n",
      "   [-0.    0.    0.01 ... -0.19 -0.07 -0.02]\n",
      "   [ 0.    0.   -1.   ...  0.3  -0.05  0.99]]\n",
      "\n",
      "  [[-0.09  0.   -0.02 ... -0.   -0.    0.  ]\n",
      "   [-0.03  0.   -0.   ... -0.   -0.    0.  ]\n",
      "   [ 0.54  0.    0.54 ...  0.15  0.08  0.  ]\n",
      "   [ 0.21 -0.03  0.   ... -0.01  0.03  0.  ]\n",
      "   [-0.98 -0.01  0.   ... -0.    0.01  0.  ]\n",
      "   [-0.    1.    0.   ...  0.32 -0.93 -0.01]]\n",
      "\n",
      "  ...\n",
      "\n",
      "  [[ 0.46 -0.03  0.54 ...  0.15  0.01  0.  ]\n",
      "   [ 0.27 -0.01  0.11 ...  0.03  0.    0.  ]\n",
      "   [-0.09  0.17 -0.16 ... -0.04 -0.06  0.  ]\n",
      "   [-0.06  0.88  0.01 ...  0.   -0.93 -0.04]\n",
      "   [ 0.29  0.19  0.   ...  0.   -0.2  -0.01]\n",
      "   [ 0.   -0.3  -0.32 ...  0.    0.26  0.33]]\n",
      "\n",
      "  [[ 0.17  0.07  0.01 ...  0.    0.2   0.  ]\n",
      "   [ 0.1   0.02  0.   ...  0.    0.04  0.  ]\n",
      "   [ 0.05 -0.5   0.02 ... -0.    0.1   0.  ]\n",
      "   [ 0.01  0.35 -0.03 ...  0.93  0.    0.09]\n",
      "   [-0.05  0.07 -0.01 ...  0.2   0.    0.02]\n",
      "   [-0.    0.05  0.93 ... -0.26  0.   -0.93]]\n",
      "\n",
      "  [[ 0.13  0.    0.05 ...  0.01  0.01  0.  ]\n",
      "   [ 0.05  0.    0.01 ...  0.    0.    0.  ]\n",
      "   [-0.53 -0.01 -0.54 ... -0.15 -0.07  0.  ]\n",
      "   [-0.21  0.1  -0.   ...  0.04 -0.09  0.  ]\n",
      "   [ 0.97  0.02 -0.   ...  0.01 -0.02  0.  ]\n",
      "   [ 0.   -0.99  0.01 ... -0.33  0.93  0.  ]]]\n",
      "\n",
      "\n",
      " [[[ 0.3   0.    0.3  ...  0.09 -0.17  0.  ]\n",
      "   [-0.49 -0.01 -0.49 ... -0.14 -0.11  0.  ]\n",
      "   [-0.   -0.   -0.   ... -0.    0.    0.  ]\n",
      "   [ 0.    0.   -0.   ... -0.8  -0.54 -0.07]\n",
      "   [ 0.    0.   -0.   ... -0.5   0.84 -0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.  ]]\n",
      "\n",
      "  [[ 0.    0.19 -0.02 ... -0.   -0.    0.  ]\n",
      "   [-0.    0.04 -0.   ... -0.   -0.    0.  ]\n",
      "   [ 0.54  0.01  0.54 ...  0.15  0.08  0.  ]\n",
      "   [ 0.    0.    0.   ... -0.01  0.03  0.  ]\n",
      "   [ 0.    0.    0.   ... -0.    0.01  0.  ]\n",
      "   [ 0.    0.    0.   ...  0.32 -0.93 -0.01]]\n",
      "\n",
      "  [[ 0.3   0.    0.3  ...  0.09 -0.17  0.  ]\n",
      "   [-0.49 -0.   -0.49 ... -0.14 -0.11  0.  ]\n",
      "   [ 0.01  0.    0.01 ...  0.   -0.01  0.  ]\n",
      "   [ 0.   -0.    0.   ... -0.8  -0.54 -0.07]\n",
      "   [ 0.   -0.    0.   ... -0.5   0.84 -0.  ]\n",
      "   [-0.   -0.    0.   ... -0.03 -0.01 -0.  ]]\n",
      "\n",
      "  ...\n",
      "\n",
      "  [[-0.08 -0.34 -0.1  ... -0.03  0.14  0.  ]\n",
      "   [ 0.15 -0.42  0.16 ...  0.04  0.09  0.  ]\n",
      "   [-0.   -0.07 -0.   ... -0.    0.    0.  ]\n",
      "   [ 0.8   0.01  0.8  ...  0.    0.21 -0.78]\n",
      "   [ 0.5   0.    0.5  ...  0.   -0.25 -0.5 ]\n",
      "   [-0.   -0.32  0.03 ...  0.    0.   -0.03]]\n",
      "\n",
      "  [[ 0.   -0.45  0.01 ... -0.    0.05  0.  ]\n",
      "   [-0.03  0.35 -0.02 ...  0.   -0.09  0.  ]\n",
      "   [-0.   -0.03  0.   ...  0.    0.    0.  ]\n",
      "   [ 0.54 -0.03  0.54 ... -0.21  0.   -0.54]\n",
      "   [-0.84 -0.01 -0.84 ...  0.25  0.    0.84]\n",
      "   [-0.    0.93  0.01 ... -0.    0.   -0.01]]\n",
      "\n",
      "  [[-0.3  -0.04 -0.3  ... -0.09  0.18  0.  ]\n",
      "   [ 0.49 -0.01  0.49 ...  0.14  0.11  0.  ]\n",
      "   [-0.01 -0.01 -0.01 ... -0.    0.01  0.  ]\n",
      "   [ 0.07 -0.    0.07 ...  0.78  0.54  0.  ]\n",
      "   [ 0.   -0.    0.   ...  0.5  -0.84  0.  ]\n",
      "   [-0.    0.01  0.   ...  0.03  0.01  0.  ]]]\n",
      "\n",
      "\n",
      " ...\n",
      "\n",
      "\n",
      " [[[ 0.09  0.    0.09 ... -0.03  0.14  0.  ]\n",
      "   [-0.14 -0.01 -0.14 ...  0.04  0.09  0.  ]\n",
      "   [-0.   -0.   -0.   ...  0.   -0.    0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.2  -0.78]\n",
      "   [ 0.    0.    0.   ... -0.   -0.22 -0.5 ]\n",
      "   [ 0.    0.    0.   ... -0.   -0.    0.  ]]\n",
      "\n",
      "  [[ 0.    0.06 -0.   ...  0.15  0.01  0.  ]\n",
      "   [-0.    0.01 -0.   ...  0.03  0.    0.  ]\n",
      "   [ 0.15  0.01  0.15 ... -0.04 -0.06  0.  ]\n",
      "   [ 0.    0.    0.   ...  0.   -0.93 -0.04]\n",
      "   [ 0.    0.    0.   ...  0.   -0.2  -0.01]\n",
      "   [ 0.    0.    0.   ...  0.    0.26  0.33]]\n",
      "\n",
      "  [[ 0.09  0.    0.09 ... -0.03  0.14  0.  ]\n",
      "   [-0.14 -0.01 -0.14 ...  0.04  0.09  0.  ]\n",
      "   [ 0.    0.    0.   ... -0.    0.    0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.21 -0.78]\n",
      "   [ 0.    0.    0.   ...  0.   -0.25 -0.5 ]\n",
      "   [ 0.    0.    0.   ...  0.    0.   -0.03]]\n",
      "\n",
      "  ...\n",
      "\n",
      "  [[-0.03  0.03 -0.03 ...  0.09 -0.04  0.  ]\n",
      "   [ 0.04 -0.04  0.04 ... -0.14 -0.03  0.  ]\n",
      "   [-0.    0.   -0.   ...  0.01 -0.15  0.  ]\n",
      "   [-0.   -0.   -0.   ...  0.   -0.54  0.21]\n",
      "   [ 0.   -0.   -0.   ...  0.    0.84  0.18]\n",
      "   [ 0.   -0.   -0.   ...  0.   -0.05  0.88]]\n",
      "\n",
      "  [[ 0.47  0.08  0.48 ...  0.    0.09  0.  ]\n",
      "   [ 0.28 -0.23  0.28 ... -0.   -0.14  0.  ]\n",
      "   [-0.33  0.   -0.33 ...  0.    0.01  0.  ]\n",
      "   [-0.2   0.93 -0.21 ...  0.54  0.    0.21]\n",
      "   [ 0.22  0.2   0.25 ... -0.84  0.   -0.32]\n",
      "   [ 0.   -0.26 -0.   ...  0.05  0.    0.02]]\n",
      "\n",
      "  [[-0.06 -0.42 -0.07 ...  0.03 -0.05  0.  ]\n",
      "   [ 0.15 -0.27  0.15 ... -0.05 -0.04  0.  ]\n",
      "   [-0.02 -0.07 -0.02 ...  0.   -0.2   0.  ]\n",
      "   [ 0.78  0.04  0.78 ... -0.21 -0.21  0.  ]\n",
      "   [ 0.5   0.01  0.5  ... -0.18  0.32  0.  ]\n",
      "   [-0.   -0.33  0.03 ... -0.88 -0.02  0.  ]]]\n",
      "\n",
      "\n",
      " [[[-0.17 -0.02 -0.17 ...  0.14  0.05  0.  ]\n",
      "   [-0.11  0.1  -0.11 ...  0.09 -0.09  0.  ]\n",
      "   [ 0.    0.    0.   ... -0.   -0.    0.  ]\n",
      "   [ 0.    0.    0.   ...  0.   -0.   -0.54]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.84]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.  ]]\n",
      "\n",
      "  [[-0.   -0.18 -0.   ...  0.01  0.2   0.  ]\n",
      "   [-0.   -0.04 -0.   ...  0.    0.04  0.  ]\n",
      "   [ 0.08 -0.11  0.08 ... -0.06  0.1   0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.09]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.02]\n",
      "   [ 0.    0.    0.   ...  0.   -0.   -0.93]]\n",
      "\n",
      "  [[-0.17 -0.02 -0.17 ...  0.14  0.05  0.  ]\n",
      "   [-0.11  0.1  -0.11 ...  0.09 -0.09  0.  ]\n",
      "   [-0.01  0.   -0.01 ...  0.    0.    0.  ]\n",
      "   [ 0.    0.    0.   ...  0.   -0.   -0.54]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.84]\n",
      "   [ 0.    0.    0.   ...  0.   -0.   -0.01]]\n",
      "\n",
      "  ...\n",
      "\n",
      "  [[ 0.05 -0.09  0.05 ... -0.04  0.09  0.  ]\n",
      "   [ 0.03  0.12  0.04 ... -0.03 -0.14  0.  ]\n",
      "   [ 0.19 -0.03  0.2  ... -0.15  0.01  0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.21]\n",
      "   [ 0.    0.    0.   ...  0.   -0.   -0.32]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.02]]\n",
      "\n",
      "  [[-0.01  0.16 -0.01 ... -0.   -0.17  0.  ]\n",
      "   [-0.01  0.11 -0.   ... -0.   -0.11  0.  ]\n",
      "   [-0.01  0.1  -0.   ... -0.   -0.11  0.  ]\n",
      "   [ 0.   -0.    0.   ... -0.    0.   -0.1 ]\n",
      "   [-0.   -0.   -0.   ...  0.    0.   -0.01]\n",
      "   [-0.    0.    0.   ... -0.    0.    0.99]]\n",
      "\n",
      "  [[ 0.12 -0.27  0.12 ... -0.15  0.    0.  ]\n",
      "   [ 0.09  0.37  0.09 ... -0.09 -0.    0.  ]\n",
      "   [ 0.59 -0.03  0.59 ...  0.15 -0.    0.  ]\n",
      "   [ 0.54 -0.09  0.54 ... -0.21  0.1   0.  ]\n",
      "   [-0.84 -0.02 -0.84 ...  0.32  0.01  0.  ]\n",
      "   [-0.    0.93  0.01 ... -0.02 -0.99  0.  ]]]\n",
      "\n",
      "\n",
      " [[[ 0.    0.    0.   ...  0.    0.    0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.   -0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.   -0.  ]]\n",
      "\n",
      "  [[ 0.    0.    0.   ...  0.    0.    0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.  ]]\n",
      "\n",
      "  [[ 0.    0.    0.   ...  0.    0.    0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.   -0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.  ]]\n",
      "\n",
      "  ...\n",
      "\n",
      "  [[ 0.    0.    0.   ...  0.    0.    0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.   -0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.   -0.  ]]\n",
      "\n",
      "  [[ 0.    0.    0.   ...  0.    0.    0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.    0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.   -0.  ]\n",
      "   [ 0.    0.    0.   ...  0.    0.   -0.  ]]\n",
      "\n",
      "  [[-0.    0.    0.   ...  0.   -0.    0.  ]\n",
      "   [-0.   -0.   -0.   ... -0.    0.    0.  ]\n",
      "   [-0.    0.   -0.   ... -0.    0.    0.  ]\n",
      "   [-0.   -0.   -0.   ...  0.   -0.    0.  ]\n",
      "   [ 0.   -0.    0.   ... -0.    0.    0.  ]\n",
      "   [ 0.   -0.   -0.   ...  0.    0.    0.  ]]]]\n"
     ]
    }
   ],
   "source": [
    "# We can view the tensor, but it's very large! It has 49 6x7 slices\n",
    "print(np.round(pd3, 2))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(7, 7, 7, 7, 6, 7)\n"
     ]
    }
   ],
   "source": [
    "# We can work out any order of the differential kinematics, but the\n",
    "# higher we go, the loger it will take\n",
    "# Lets do the fifth-order differential dinematics\n",
    "pd5 = partial_fkine0(ets, q, 5)\n",
    "\n",
    "# View the dimension\n",
    "print(pd5.shape)"
   ]
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "528815e074ebcdb9b34bcb695d4aa9d425bdb2cc6656b4ca45050b51a4125937"
  },
  "kernelspec": {
   "display_name": "Python 3.8.10 ('rtb')",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.0"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
